#! /usr/bin/env python
# -*- coding: UTF-8 -*-
import rospy
import actionlib
from objectpoint.msg import *
from Class.Calcuation import *
import numpy as np

class MyActionServer:
    def __init__(self):

        #SimpleActionServer(name, ActionSpec, execute_cb=None, auto_start=True)
        self.server = actionlib.SimpleActionServer("Point-serve",PointCalculaterAction,self.cb,False)
        self.server.start()
        self.Pt = PointTransform() # Transeformer have been init.
        rospy.loginfo("服务端启动")


    def cb(self,goal):

        rospy.loginfo("服务端处理请求:")
        #1.解析目标值

        u = goal.u
        v = goal.v
        d = goal.d
        update = goal.update

        if update:
            self.Pt.get_transform()
        
        point3d = np.array(Point2DTo3D([u,v],d)) # Link:Carame
        point3d = self.Pt.transformation(point3d/1000) # Link:PandaLink0
        x,y,z = point3d

        result = PointCalculaterResult()
        result.x = x
        result.y = y
        result.z = z
        self.server.set_succeeded(result)
        rospy.loginfo("响应")
        
if __name__ == "__main__":
    rospy.init_node("action_server_p")
    server = MyActionServer()
    rospy.spin()
